/**
 * @file AX_12.c
 * @author  Nguyen Ngoc The Vinh < vsop_valentino@yahoo.com>
 * @author  Nguyen Ngoc Hung <>
 * @date 30-10-2013
 *
 * @copyright
 * This poject and all of its relevant documents, source codes, compiled libraries are belong
 * to <b> Smart Sensing and Intelligent Controlling Group (SSAIC)</b> \n
 * You are prohibited to broadcast, distribute, copy, modify, print, or reproduce this in anyway
 * without written permission from SSAIC \n
 * <b> Copyright (C) 2012 by SSAIC, All Right Reversed </b>
 *
 * @brief
 * Implement hardware abstract layer to control AX-12 servo
 * @attention
 */

#ifndef	__AX_12_H
#define	__AX_12_H

/*** Define AX_12 constant  ***/
#define AX12_FAIL					0
#define AX12_OK						1
#define AX12_COLLISION				2	
#define AX12_NO_RESPONSE			3	
#define AX12_WRONG_ID				4	
#define AX12_CHECKSUM_ERROR			5

/*** Define AX_12 error  ***/
#define AX12_Input_Voltage_Error	0		
#define AX12_Angle_Limit_Error		1
#define AX12_Over_Heating_Error		2
#define AX12_Range_Error			3
#define AX12_Check_Sum_Error		4
#define AX12_Over_Load_Error		5
#define AX12_Instruction_Error		6

/*** Define servo's register addresses ***/
#define AX12_MODEL_NUM_H_REG				0x00
#define AX12_MODEL_NUM_L_REG				0x01
#define AX12_FIRM_VER_REG					0x02
#define AX12_ID_REG							0x03
#define AX12_BAUD_RATE_REG					0x04
#define AX12_RETURN_DELAY_TIME_REG			0x05
#define AX12_CW_ANGLE_LIMIT_L_REG			0x06
#define AX12_CW_ANGLE_LIMIT_H_REG			0x07
#define AX12_CCW_ANGLE_LIMIT_L_REG			0x08	
#define AX12_CCW_ANGLE_LIMIT_H_REG			0x09
#define AX12_Highest_Limit_Temperature_REG	0x0B
#define AX12_Lowest_Limit_Voltage_REG		0x0C
#define AX12_Highest_Limit_Voltage_REG		0x0D
#define AX12_Max_Torque_L_REG				0x0E
#define AX12_Min_Torque_H_REG				0x0F
#define AX12_Status_Return_Level_REG		0x10
#define AX12_Alarm_Led_REG					0x11
#define AX12_Alarm_Shutdown_REG				0x12
#define AX12_Down_Calibration_L_REG			0x14
#define AX12_Down_Calibration_H_REG			0x15
#define AX12_Up_Calibration_L_REG			0x16
#define AX12_Up_Calibration_H_REG			0x17
#define AX12_Torque_Enable_REG				0x18
#define AX12_LED_REG						0x19
#define AX12_CW_Compliance_Margin_REG		0x1A
#define AX12_CCW_Compliance_Margin_REG		0x1B
#define AX12_CW_Compliance_Slope_REG		0x1C
#define AX12_CCW_Compliance_Slope_REG		0x1D
#define AX12_Goal_Position_L_REG			0x1E
#define AX12_Goal_Position_H_REG			0x1F
#define AX12_Moving_Speed_L_REG				0x20
#define AX12_Moving_Speed_H_REG				0x21
#define AX12_Torque_Limit_L_REG				0x22
#define AX12_Torque_Limit_H_REG				0x23
#define AX12_Present_Position_L_REG			0x24
#define AX12_Present_Position_H_REG			0x25
#define AX12_Present_Speed_L_REG			0x26
#define AX12_Present_Speed_H_REG			0x27
#define AX12_Present_Load_L_REG				0x28
#define AX12_Present_Load_H_REG				0x29
#define AX12_Present_Voltage_REG			0x2A
#define AX12_Present_Temperature_REG		0x2B
#define AX12_Registered_Instruction_REG		0x2C	
#define AX12_Moving_REG						0x2E
#define AX12_Lock_REG						0x2F
#define AX12_Punch_L_REG					0x30
#define AX12_Punch_H_REG					0x31


/*** Define servo's instructions ***/
#define AX12_PING_INSTR				0x01
#define AX12_READ_DATA_INSTR		0x02
#define AX12_WRITE_DATA_INSTR		0x03
#define AX12_WRITE_REG_INSTR		0x04
#define AX12_ACTION_INSTR			0x05
#define AX12_RESET_INSTR			0x06
#define AX12_SYNC_WRITE_INSTR		0x83

//=========================================================//
//====================Public functions=====================//
//=========================================================//
class	AX_12 {
public:
/* SET values functions */ 
	uint8_t		SetID(uint8_t servoId, uint8_t ID);												/** tested */
	uint8_t		SetBaudRate(uint8_t servoId, uint8_t BaudRate);
	uint8_t		SetReturnDelayTime(uint8_t servoId, uint8_t delayTime); 
	uint16_t	SetCWAngleLimit(uint8_t servoId, uint16_t angleLimit);							/** tested */
	uint16_t	SetCCWAngleLimit(uint8_t servoId, uint16_t angleLimit);							/** tested */

	uint8_t		SetTemperatureLimit(uint8_t servoId, uint8_t temperatureLimit);					/** tested */
	uint8_t		SetLowestVoltageLimit(uint8_t servoId, uint8_t voltageLimit);
	uint8_t		SetHighestVoltageLimit(uint8_t servoId, uint8_t voltageLimit);
	uint16_t	SetMaxTorque(uint8_t servoId, uint16_t maxTorque);								/** tested */
	uint8_t		SetStatusReturnLevel(uint8_t servoId, uint8_t returnLevel);

	uint8_t		SetAlarmLed(uint8_t servoId, uint8_t alarmLed);									/** tested */
	uint8_t		SetAlarmShutdown(uint8_t servoId, uint8_t alarmShutdown);						/** tested */
	uint8_t		SetTorqueEnable(uint8_t servoId, uint8_t torqueEnable);							/** tested */
	uint8_t		SetLed(uint8_t servoId, uint8_t ledVal);										/** tested */
	uint8_t		SetCWComplianceMargin(uint8_t servoId, uint8_t complianceMargin);

	uint8_t		SetCCWComplianceMargin(uint8_t servoId, uint8_t complianceMargin);
	uint8_t		SetCWComplianceSlope(uint8_t servoId, uint8_t complianceSlope);
	uint8_t		SetCCWComplianceSlope(uint8_t servoId, uint8_t complianceSlope);
	uint16_t	SetGoalPosition(uint8_t servoId, uint16_t goalPosition);						/** tested */
	uint16_t	SetMovingSpeed(uint8_t servoId, uint16_t movingSpeed);							/** tested */

	uint16_t	SetTorqueLimit(uint8_t servoId, uint16_t torqueLimit);							/** tested */
	uint8_t		SetRegisteredInstruction(uint8_t servoId, uint8_t instruction);
	uint8_t		SetLock(uint8_t servoId, uint8_t lockMode);										/** tested */
	uint16_t	SetPunch(uint8_t servoId, uint16_t punch);										/** tested */

/* GET values functions */

	uint16_t	GetModelNum(uint8_t servoId);
	uint8_t		GetFirmwareVersion(uint8_t servoId);
	uint8_t		GetId(uint8_t servoId);
	uint8_t		GetBaudRate(uint8_t servoId);
	uint8_t		GetReturnDelayTime(uint8_t servoId);

	uint16_t	GetCWAngleLimit(uint8_t servoId);
	uint16_t	GetCCWAngleLimit(uint8_t servoId);
	uint8_t		GetTemperatureLimit(uint8_t servoId);
	uint8_t		GetLowestVoltageLimit(uint8_t servoId);
	uint8_t		GetHighestVoltageLimit(uint8_t servoId);

	uint16_t	GetMaxTorque(uint8_t servoId);
	uint8_t		GetStatusReturnLevel(uint8_t servoId);
	uint8_t		GetAlarmLED(uint8_t servoId);
	uint8_t		GetAlarmShutdown(uint8_t servoId);
	uint16_t	GetDownCalibration(uint8_t servoId);

	uint16_t	GetUpCalibration(uint8_t servoId);
	uint8_t		GetTorqueEnable(uint8_t servoId);
	uint8_t		GetLed(uint8_t servoId);
	uint8_t		GetCWComplianceMargin(uint8_t servoId);
	uint8_t		GetCCWComplianceMargin(uint8_t servoId);

	uint8_t		GetCWComplianceSlope(uint8_t servoId);
	uint8_t		GetCCWComplianceSlope(uint8_t servoId);
	uint16_t	GetGoalPosition(uint8_t servoId);
	uint16_t	GetMovingSpeed(uint8_t servoId);
	uint16_t	GetTorqueLimit(uint8_t servoId);

	uint16_t	GetPresentPosition(uint8_t servoId);
	uint16_t	GetPresentSpeed(uint8_t servoId);
	uint16_t	GetPresentLoad(uint8_t servoId);
	uint8_t		GetPresentVoltage(uint8_t servoId);
	uint8_t		GetPresentTemperature(uint8_t servoId);

	uint8_t		GetRegisteredInstruction(uint8_t servoId);
	uint8_t		GetMoving(uint8_t servoId);
	uint8_t		GetLock(uint8_t servoId);
	uint16_t	GetPunch(uint8_t servoId);


/*** Support Functions ***/
	void	CheckCollision(uint8_t* status);
	void	WaitResponse(uint8_t* status);
	void	CheckSum(uint8_t servoId, uint8_t length, uint8_t instruction, uint8_t parameter[], uint8_t sum, uint8_t* status);
	void	CheckRecvId(uint8_t servoId, uint8_t* status);
	void	AssignValueStatus(uint8_t *status);
	void	Write(uint8_t servoId, uint8_t address, uint8_t value);											/** tested */
	void	Write_16bits(uint8_t servoId, uint8_t address, uint16_t value);									/** tested */
	void	Read(uint8_t servoId, uint8_t address, uint8_t Len);
	friend	void	snack();

private:
	uint8_t		SendPing(uint8_t servoId, uint8_t* status);
	uint8_t		SendReadDataOneByte(uint8_t servoId, uint8_t address);
	uint16_t    SendReadDataTwoByte(uint8_t servoId, uint8_t address);

	uint8_t  	SendWriteDataOneByte(uint8_t servoId, uint8_t address, uint8_t value);						/** tested */
	uint8_t  	SendWriteDataTwoBytes(uint8_t servoId, uint8_t address, uint16_t value);					/** tested */
	uint8_t  	SendWriteDataMultiBytes(uint8_t servoId, uint8_t address, uint8_t value[], uint8_t length);	/** tested */

	uint8_t  	SendWriteRegOneByte(uint8_t servoId, uint8_t address, uint8_t value);						/** tested */
	uint8_t  	SendWriteRegTwoBytes(uint8_t servoId, uint8_t address, uint16_t value);						/** tested */
	uint8_t  	SendWriteRegMultiBytes(uint8_t servoId, uint8_t address, uint8_t value[], uint8_t length);	/** tested */

	uint8_t		SendAction(uint8_t servoId);																/** tested */
	uint8_t		SendReset(uint8_t servoId);																	/** tested */
	uint8_t		SendSyncWrite(uint8_t value[], uint8_t length);
	uint8_t     SendSyncWriteOneByte(uint16_t value[], uint8_t address, uint8_t numServo);					/** tested */
	uint8_t     SendSyncWriteTwoByte(uint16_t value[], uint8_t address, uint8_t numServo);					/** tested */

};

#endif
